#include "stewart.h"
#include <chrono>

using namespace std;
using namespace Eigen;

int main(int argc, char  *argv[])
{
    stewart ste(500,124,40.0*M_PI/180,87.0*M_PI/180);
    ste.up_down_P_calcu(ste._pDown,ste._pUp,ste._DownR,ste._UpR,ste._down_alpha,ste._up_beta);

    Eigen::VectorXd initPose(6);
    initPose<<0,0,738.472,0.0*M_PI/180.0,0.0*M_PI/180.0,  0.0*M_PI/180.0;//任意设置的初始值，这里627.472是0位时的实际位姿

    Eigen::VectorXd konwn_length(6);//正运动学已知量
    // konwn_length<<741,736.5,728.6,731,746.5,749;//对应20,0,627.472,0.0*M_PI/180.0,0.0*M_PI/180.0,  0.0*M_PI/180.0;
        konwn_length<<741.1,712,707.7,742.8,771.1,762.6;//对应20,0,627.472,10.0*M_PI/180.0,10.0*M_PI/180.0,  0.0*M_PI/180.0;
        // konwn_length<<699.9,699.9,715.1,848.6,848.6,715.1;//对应0,0,627.472,60.0*M_PI/180.0,0.0*M_PI/180.0,  0.0*M_PI/180.0;

    // konwn_length = inverse_fun(desPP,500.0,124.0,40.0*M_PI/180,87.0*M_PI/180);
 
    Eigen::VectorXd inv_result(6);
    inv_result = ste.inverse_fun2(initPose,konwn_length,ste._pDown,ste._pUp);
    cout<<"inv_result +konwn_length"<<inv_result +konwn_length<<endl;
    double esp=0.0001;//求解精度
    double E=1000;//差值
    int mk = 0;
    auto s = std::chrono::system_clock::now();


    while (abs(E)> esp)
    {
            //inverse
            // inv_result = inverse_fun(desPP,500.0,124.0,40.0*M_PI/180,87.0*M_PI/180);
            inv_result = ste.inverse_fun2(initPose,konwn_length,ste._pDown,ste._pUp);

            // //jacobian   
            Eigen::Matrix<double,6,6> jacobi= ste.jaco_calcu2(ste._pDown,ste._pUp,initPose);
            // cout<<"jaco"<<jacobi<<endl;
            Eigen::VectorXd X(6),x_temp(6),pose_temp(6);//(A^T*A)^-1)A^T
            // x_temp =(jacobi.transpose()*jacobi).inverse()* jacobi.transpose()*inv_result;
            x_temp =jacobi.inverse()*inv_result;

            X=initPose-x_temp;
            E=sqrt((X-initPose).transpose()*(X-initPose));
            // cout<<"(X-X_init).norm"<<endl<<(X-X_init)<<endl;
            initPose=X;
            std::cout<<"E"<<endl<<E<<endl;
            std::cout<<"X"<<endl<<X<<endl;
            mk++;
    }
    std::cout<<"mk"<<endl<<mk<<endl;
        //work
    auto e = std::chrono::system_clock::now();
    std::chrono::duration<double> mid1 = e - s;
    std::cout << mid1.count() << std::endl;//间隔时间(s)
    return 0;
}


